package roborally;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

/**
 * A class shortestPathAlgorithm, that calculates the shortest path between 2 positions using notes. An explanation of this class
 * is given as a comment below the definition.
 * 
 * (We know that the name of this class should start with a capital letter, but the refactoring of Eclipse did not allow us to do so.)
 * 
 * @version  2.90
 * @author   Michiel Ruelens & Dieter Naten
 */
public class shortestPathAlgorithm {

	
	private static List<Node> stillToDoNodes = new ArrayList<Node>(); //Possible nodes to be handled in the next step
	private static List<Node> doneNodes  = new ArrayList<Node>(); //Handled nodes
	private static Node bestTargetNode = null; //The node closest to the goal
	private static List<Node> shortestPath = new ArrayList<Node>(); //The shortest path to bestTargetNode
	private static boolean cancelled = false; //Boolean to stop the while-loop.

	/**
	 * This method calculates the shortest path from c1 to c2. It will return an array containing all the nodes
	 * that make the path from c1 to c2. If there is no way of reaching c2 from c1 then it will return all the nodes
	 * that make a path to the closest possible position to c2.
	 * 
	 * @param	robot1
	 * 			The first robot of two between which you want to calculate the shortest path
	 * @param	c2
	 * 			The coordinate to which robot1 should move as efficient as possible
	 * @param	startOrientation
	 * 			The orientation to start with for searching the shortetst path
	 * @param	energy
	 * 			The energy of the robot that has to be moved following the shortest path
	 * @param	board
	 * 			The board on which the path should be calculated
	 * @pre		| Orientation.isValidOrientation(startOrientation)
	 * @pre		| Coordinate.isValidXCoordinate(board, c2.getX())
	 * @pre		| Coordinate.isValidYCoordinate(board, c2.getY())
	 * @return	The list with nodes that forms the shortest path between 2 coordinates. If there is no way of reaching c2 from c1 
	 * 			then it will return all the nodes that make a path to the closest possible position to c2. 
	 * 			| result ==
	 * 			| NOT EXISTS 
	 * 			| path = List<Node> WHERE
	 *			| path.get(path.size-1).getGscore <= bestPath.get(bestPath.size-1).getGscore &&
	 *			| path.get(path.size-1).getEnergyToReach < bestPath.get(bestPath.size-1).getEnergyToReach
	 */
	public static List<Node> getShortestPath(Robot robot1 , Coordinate c2, Orientation startOrientation, double energy, Board board) {
		assert(Orientation.isValidOrientation(startOrientation) && Coordinate.isValidXCoordinate(board, c2.getX()) && 
				Coordinate.isValidYCoordinate(board, c2.getY()));
		Coordinate c1 = robot1.getCoordinate();
		List<Node> bestPath = new ArrayList<Node>();
		if(c1==null||c2==null){
			System.out.println("One of 2 coordinates entered to calculate a path between is null.");
		}
		else{
			Node startNode = Node.getNode(c1, (long) 0, Coordinate.getManhattanDistance(c1,c2) , (long) 0 , null, startOrientation);
		
			//Initialises the 4 nodes around the startNode and adds them to the stillToDoNodes list.
			stillToDoNodes.add(startNode);
			bestTargetNode = startNode;
		
			while(!stillToDoNodes.isEmpty() && !cancelled){
//				for(Node n: stillToDoNodes){
//					System.out.println("STILLTODONODE: "+n.getCoordinate().getX()+" "+n.getCoordinate().getY());
//				}
				Node lowestScoreNode = getNodeWithLowestFScore(stillToDoNodes);
//				System.out.println("LSN: "+lowestScoreNode.getCoordinate().getX()+" "+lowestScoreNode.getCoordinate().getY());
				if((lowestScoreNode.getHScore()<bestTargetNode.getHScore()) 
						||(lowestScoreNode.getHScore()==bestTargetNode.getHScore() 
						&& lowestScoreNode.getGScore()<bestTargetNode.getGScore())) {		
					bestTargetNode = lowestScoreNode;
					}
				if(!(lowestScoreNode.getHScore()==0)){
//					boolean remove = 
					stillToDoNodes.remove(lowestScoreNode);
//					System.out.println(remove);
					stillToDoNodes.addAll(calculateNeighborNodes(lowestScoreNode, robot1, c2));
					doneNodes.add(lowestScoreNode);				
				}
				else{
					bestTargetNode = lowestScoreNode;
					cancelled = true;
				}
			}
			bestPath = bestTargetNode.getPath();
			Collections.reverse(bestPath);
		}
		return bestPath;
	}

	/**
	 * Calculates the scores for the nodes around the lowestFScoreNode and returns them as a list.
	 * Note that the List does not nessecarily have 4 nodes in it. If there are walls or nodes that
	 * are already done, this list will contain fewer than 4 nodes. It can never have more than 4 nodes.
	 */
	private static List<Node> calculateNeighborNodes(Node node, Robot startRobot, Coordinate targetCoordinate) {
		Board board = startRobot.getBoard();
		Coordinate startCoordinate = startRobot.getCoordinate();
		Orientation startOrientation = Orientation.convertIntToOrientation(startRobot.getOrientation());
		Double energy = startRobot.getEnergy();
		List<Node> knownNodesAroundNode = getKnownNodesAroundNode(node, board);
		List<Coordinate> coordinateList = Coordinate.getFourCoordinatesAroundCoordinateThatAreNotWallsAndOnBoard(node.getCoordinate(), board);
		
		List<Node> neighborNodes = new ArrayList<Node>();
		
		for(Coordinate c: coordinateList){
			boolean match = false;
			if(!knownNodesAroundNode.isEmpty()){
				for(Node n : knownNodesAroundNode){
					if(n.getCoordinate().equals(c)){
						match = true;
//						System.out.println("KNOWN NODE: "+n.getCoordinate().getX()+" "+n.getCoordinate().getY());
					}
				}
			}
			if(match==true){
				Long gScore = node.getGScore() + 1;
				Node existingNode = Node.getNode(c,(long)0,(long)0,(long)0,null,Orientation.TOP);//get the existing node, only the coordinate matters as this coordinate already exists.
				//The node already existed, but a shorter path to this node has just been found, so it gets updated.
				if(gScore<existingNode.getGScore()){
					existingNode.changeGScore(gScore);
					existingNode.changeParentNode(node);
				}
			}
			else{
				Node parent = node;
				Long gScore = parent.getGScore() + 1;
				Long hScore = Coordinate.getManhattanDistance(c, targetCoordinate);
				Long energyToReach = parent.getEnergyToReach() + Energy.getEnergyRequiredToTravelOneStep(parent.getCoordinate(), c, parent.getOrientation(), startRobot);
				if(gScore*Robot.StandardCostToMove<=energy){
					Orientation orient = null;
					if(c.getX()>parent.getCoordinate().getX()){
						orient = Orientation.RIGHT;
					}
					if(c.getX()<parent.getCoordinate().getX()){
						orient = Orientation.LEFT;
					}
					if(c.getY()>parent.getCoordinate().getY()){
						orient = Orientation.BOTTOM;
					}
					if(c.getY()<parent.getCoordinate().getY()){
						orient = Orientation.TOP;
					}
					Node newNode = Node.getNode(c,gScore, hScore , energyToReach, parent, orient);
//					System.out.println("node= "+node);
//					System.out.println("ADDED NEW NODE: "+newNode.getCoordinate().getX()+" "+newNode.getCoordinate().getY()+" "+newNode.getParentNode());
					neighborNodes.add(newNode);
				}
			}
		}
//		for(Node n: neighborNodes){
//			System.out.println("NEW NEIGHBOR: "+n.getCoordinate().getX()+" "+n.getCoordinate().getY());
//		}
		return neighborNodes;
	}

	/**
	 * Returns the node with the lowest FScore from the given List. If FScores are equal for certain nodes then
	 * GScores and HScores will be taken into account to determine which node is returned. If multiple nodes are
	 * completely equal in scores, a random of said nodes will be returned.
	 */
	private static Node getNodeWithLowestFScore(List<Node> nodeList) {
		Collections.reverse(nodeList);
		Node resultNode = null;
		if(nodeList.isEmpty()){
			System.out.println("method: getNodeWithLowestScore - parameter nodeList is empty.");
		}
		else if(nodeList.size()==1){
			resultNode = nodeList.get(0);
//			System.out.println("SIZE=1 : "+resultNode.getCoordinate().getX()+" "+resultNode.getCoordinate().getY());
		}
		else{
			resultNode = nodeList.get(0);
			for(Node n: nodeList){
//				System.out.println(n.getCoordinate().getX()+" "+n.getCoordinate().getY());
//				System.out.println(n.getParentNode());
				if(n.getFScore()<resultNode.getFScore()){
					resultNode = n;
				}
				//Deze else zorgt ervoor dat Nodes waarnaar rechtdoor gelopen werd om ze te bereiken geprefereerd worden boven Nodes waarnaar gedraaid moest worden. M.a.w. rechtdoor>draaien
				else if(n.getFScore()==resultNode.getFScore() 
						&& (Orientation.getAmountOfTurnsRequiredToTurnFromTo(n.getParentNode().getOrientation(), n.getOrientation())
							< Orientation.getAmountOfTurnsRequiredToTurnFromTo(resultNode.getParentNode().getOrientation(), resultNode.getOrientation()))){
					resultNode = n;
				}
			}
		}
//		System.out.println("***"+resultNode.getCoordinate().getX()+" "+resultNode.getCoordinate().getY()+"***");
//		System.out.println("---------");
		return resultNode;
	}
	
	/**
	 * Returns a List containing the nodes that are already calculated around the given node.
	 */
	private static List<Node> getKnownNodesAroundNode(Node node, Board board){
		List<Node> resultList = new ArrayList<Node>();
		List<Node> allKnownNodes = new ArrayList<Node>();
		allKnownNodes.addAll(stillToDoNodes);
		allKnownNodes.addAll(doneNodes);
		
		if(!allKnownNodes.isEmpty()){		
			List<Coordinate> coordinateList = Coordinate.getFourCoordinatesAroundCoordinateThatAreNotWallsAndOnBoard(node.getCoordinate(), board);
			for(Coordinate c : coordinateList){
				for(Node n : allKnownNodes){
					if(n.getCoordinate().equals(c)){
						resultList.add(n);
					}
				}
			}
		}
		return resultList;
	}
	
	/**
	 * This method returns the number of moves a robot has to make when following the given path.
	 * 
	 * @param 	path
	 * 			The path the robot has to follow
	 * @pre		| !path.isEmpty()
	 * @return	path.get(path.size()-1).getGScore()
	 */
	public static Long getNumberOfMovesInPath(List<Node> path){
		assert(!path.isEmpty());
		Node lastNode = path.get(path.size()-1);
		Long gScore = lastNode.getGScore();
		return gScore;		
	}
	
	/**
	 * This method returns the number of turns a robot has to make when following the given path.
	 * 
	 * @param 	path
	 * 			The path the robot has to follow
	 * @pre		| !path.isEmpty()
	 * @return	| result ==
	 * 			| numberOfTurns FROM:
	 * 			| for(Node n:path){
	 *			|		if(n.getParentNode()!=null){
	 *			|			Node parent = n.getParentNode();
	 *			|		if(parent.getOrientation()!=n.getOrientation()){
	 *			|			numberOfTurns++
	 */
	public static int getNumberOfTurnsInPath(List<Node> path){
		assert(!path.isEmpty());
		int numberOfTurns = 0;
		for(Node n:path){
			if(n.getParentNode()!=null){
				Node parent = n.getParentNode();
				if(parent.getOrientation()!=n.getOrientation()){
					numberOfTurns++;
				}
			}
		}
		return numberOfTurns;
	}
	
	/**
	 * Clears the parameters of the Algorithm
	 * 
	 * @post	| stillToDoNodes.isEmpty() == true 
	 * @post	| doneNodes.isEmpty() == true 
	 * @post	| bestTargetNode == null 
	 * @post	| cancelled == false
	 * @post	| shortestPath.isEmpty() == true 
	 */
	public static void clearAlgorithm(){
		stillToDoNodes.clear();
		doneNodes.clear();
		bestTargetNode = null;
		cancelled = false;
		shortestPath.clear();
	}

	/**
	 * This method is invoked only when 2 robots are standing on top of eachother and thus one robot only needs to make 1 move.
	 * 
	 * @param 	robot
	 * 			One of both robots that stand on eachother and has to move
	 * @return  The node that is most energy-efficient to move to.
	 */
	public static List<Node> moveOneStepBecauseOnTop(Robot robot) {
		List<Node> path = new ArrayList<Node>();
		Node startNode = Node.getNode(robot.getCoordinate(),(long)0,(long)1,(long)0,null,Orientation.convertIntToOrientation(robot.getOrientation()));
		path.add(startNode);
		Coordinate c = robot.getCoordinate();
		Board b = robot.getBoard();
		Orientation o = Orientation.convertIntToOrientation(robot.getOrientation());
		Coordinate front = Coordinate.getCoordinateInFront(c, b, o);
		Coordinate right = Coordinate.getCoordinateToRight(c, b, o);
		Coordinate left = Coordinate.getCoordinateToLeft(c, b, o);
		Coordinate behind = Coordinate.getCoordinateBehind(c, b, o);
		if(!front.equals(c)){
			path.add(Node.getNode(front,(long)1,(long)0,(long)robot.getCostToMove(),
					startNode,Orientation.convertIntToOrientation(robot.getOrientation())));
		}
		else if(!right.equals(c)){
			path.add(Node.getNode(right,(long)1,(long)0,(long)robot.getCostToMove()+robot.getCostToTurn(),
					startNode,Orientation.getNextOrientationClockwise(Orientation.convertIntToOrientation(robot.getOrientation()))));
		}
		else if(!left.equals(c)){
			path.add(Node.getNode(left,(long)1,(long)0,(long)robot.getCostToMove()+robot.getCostToTurn(),
					startNode,Orientation.getNextOrientationAntiClockwise(Orientation.convertIntToOrientation(robot.getOrientation()))));
		}
		else if(!behind.equals(c)){
			path.add(Node.getNode(behind,(long)1,(long)0,(long)robot.getCostToMove()+(2*robot.getCostToTurn()),
					startNode,Orientation.getNextOrientationAntiClockwise(Orientation.getNextOrientationAntiClockwise(Orientation.convertIntToOrientation(robot.getOrientation())))));
		}
		else{
			System.out.println(robot+" could not move because all surrounding coordinates are blocked.");
		}
//		robot.moveOneStep(result);		
		return path;
	}
}
